var ROSLIB = ROSLIB || {
    REVISION : '1.3.0'
};

ROSLIB.Vector3 = function (options) {
    options = options || {};
    this.x = options.x || 0;
    this.y = options.y || 0;
    this.z = options.z || 0;
}
  
ROSLIB.Vector3.prototype.add = function(v) {
    this.x += v.x;
    this.y += v.y;
    this.z += v.z;
};

ROSLIB.Vector3.prototype.subtract = function(v) {
    this.x -= v.x;
    this.y -= v.y;
    this.z -= v.z;
};

ROSLIB.Vector3.prototype.multiplyQuaternion = function(q) {
    var ix = q.w * this.x + q.y * this.z - q.z * this.y;
    var iy = q.w * this.y + q.z * this.x - q.x * this.z;
    var iz = q.w * this.z + q.x * this.y - q.y * this.x;
    var iw = -q.x * this.x - q.y * this.y - q.z * this.z;
    this.x = ix * q.w + iw * -q.x + iy * -q.z - iz * -q.y;
    this.y = iy * q.w + iw * -q.y + iz * -q.x - ix * -q.z;
    this.z = iz * q.w + iw * -q.z + ix * -q.y - iy * -q.x;
};


ROSLIB.Quaternion = function(options) {
    options = options || {};
    this.x = options.x || 0;
    this.y = options.y || 0;
    this.z = options.z || 0;
    this.w = (typeof options.w === 'number') ? options.w : 1;
}

ROSLIB.Quaternion.prototype.conjugate = function() {
    this.x *= -1;
    this.y *= -1;
    this.z *= -1;
};

ROSLIB.Quaternion.prototype.norm = function() {
    return Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w);
};

ROSLIB.Quaternion.prototype.normalize = function() {
    var l = Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w);
    if (l === 0) {
      this.x = 0;
      this.y = 0;
      this.z = 0;
      this.w = 1;
    } else {
      l = 1 / l;
      this.x = this.x * l;
      this.y = this.y * l;
      this.z = this.z * l;
      this.w = this.w * l;
    }
};

ROSLIB.Quaternion.prototype.reverse = function() {
    var theta = 2 * Math.atan2(this.z, this.w)
    if (theta > Math.PI && theta <= (2 * Math.PI)) {
        theta -= 2 * Math.PI;
    } else if(theta < (-1 * Math.PI) && theta >= (-2 * Math.PI)) {
        theta += 2 * Math.PI;
    }
    return theta;
}

ROSLIB.Quaternion.prototype.invert = function() {
    this.conjugate();
    this.normalize();
};

ROSLIB.Quaternion.prototype.multiply = function(q) {
    var newX = this.x * q.w + this.y * q.z - this.z * q.y + this.w * q.x;
    var newY = -this.x * q.z + this.y * q.w + this.z * q.x + this.w * q.y;
    var newZ = this.x * q.y - this.y * q.x + this.z * q.w + this.w * q.z;
    var newW = -this.x * q.x - this.y * q.y - this.z * q.z + this.w * q.w;
    this.x = newX;
    this.y = newY;
    this.z = newZ;
    this.w = newW;
};

ROSLIB.Pose = function(options) {
    options = options || {};
    // copy the values into this object if they exist
    this.position = new ROSLIB.Vector3(options.position);
    this.orientation = new ROSLIB.Quaternion(options.orientation);
}

ROSLIB.Pose.prototype.applyTransform = function(tf) {
    this.position.multiplyQuaternion(tf.rotation);
    this.position.add(tf.translation);
    var tmp = tf.rotation.clone();
    tmp.multiply(this.orientation);
    this.orientation = tmp;
};

ROSLIB.Pose.prototype.clone = function() {
    return new ROSLIB.Pose(this);
};

ROSLIB.Pose.prototype.multiply = function(pose) {
    var p = pose.clone();
    p.applyTransform({ rotation: this.orientation, translation: this.position });
    return p;
};

ROSLIB.Pose.prototype.getInverse = function() {
    var inverse = this.clone();
    inverse.orientation.invert();
    inverse.position.multiplyQuaternion(inverse.orientation);
    inverse.position.x *= -1;
    inverse.position.y *= -1;
    inverse.position.z *= -1;
    return inverse;
};

export { ROSLIB }